﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;

namespace KinectDwa.Parsers
{
    public class ActivityWatcher : IComparable<ActivityWatcher>
    {
        private const float ActivityFalloff = 0.98f;

        private float activityLevel;
        private SkeletonPoint previousPosition;
        private SkeletonPoint previousDelta;

        internal ActivityWatcher(Skeleton s)
        {
            this.activityLevel = 0.0f;
            this.TrackingId = s.TrackingId;
            this.Updated = true;
            this.previousPosition = s.Position;
            this.previousDelta = new SkeletonPoint();
        }

        internal int TrackingId { get; private set; }

        internal bool Updated { get; private set; }

        public int CompareTo(ActivityWatcher other)
        {
            // Use the existing CompareTo on float, but reverse the arguments,
            // since we wish to have larger activityLevels sort ahead of smaller values.
            return other.activityLevel.CompareTo(this.activityLevel);
        }

        internal void NewPass()
        {
            this.Updated = false;
        }

        internal void Update(Skeleton s)
        {
            SkeletonPoint newPosition = s.Position;
            SkeletonPoint newDelta = new SkeletonPoint
            {
                X = newPosition.X - this.previousPosition.X,
                Y = newPosition.Y - this.previousPosition.Y,
                Z = newPosition.Z - this.previousPosition.Z
            };

            SkeletonPoint deltaV = new SkeletonPoint
            {
                X = newDelta.X - this.previousDelta.X,
                Y = newDelta.Y - this.previousDelta.Y,
                Z = newDelta.Z - this.previousDelta.Z
            };

            this.previousPosition = newPosition;
            this.previousDelta = newDelta;

            float deltaVLengthSquared = (deltaV.X * deltaV.X) + (deltaV.Y * deltaV.Y) + (deltaV.Z * deltaV.Z);
            float deltaVLength = (float)Math.Sqrt(deltaVLengthSquared);

            this.activityLevel = this.activityLevel * ActivityFalloff;
            this.activityLevel += deltaVLength;

            this.Updated = true;
        }
    }
}
